#include "blueteeth.h"
#include "motor.h"
#include <rtthread.h>

u8 rec = 0, res_ctrl;
u8 Speed = 'L';
float Speed_g;

void BlueTeeth_Init()
{
	GPIO_InitTypeDef GPIO_InitStructure;
	USART_InitTypeDef USART_InitStructure;
	NVIC_InitTypeDef NVIC_InitStructure;

	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);  //使能GPIOA时钟
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE); //使能USART1时钟

	//串口1对应引脚复用映射
	GPIO_PinAFConfig(GPIOA, GPIO_PinSource2, GPIO_AF_USART2); //GPIOA9复用为USART1
	GPIO_PinAFConfig(GPIOA, GPIO_PinSource3, GPIO_AF_USART2); //GPIOA10复用为USART1

	//USART1端口配置
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2 | GPIO_Pin_3; //GPIOA9与GPIOA10
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;		   //复用功能
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;	  //速度50MHz
	GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;		   //推挽复用输出
	GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;		   //上拉
	GPIO_Init(GPIOA, &GPIO_InitStructure);				   //初始化PA9，PA10

	//USART1 初始化设置
	USART_InitStructure.USART_BaudRate = 9600;										//波特率设置
	USART_InitStructure.USART_WordLength = USART_WordLength_8b;						//字长为8位数据格式
	USART_InitStructure.USART_StopBits = USART_StopBits_1;							//一个停止位
	USART_InitStructure.USART_Parity = USART_Parity_No;								//无奇偶校验位
	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; //无硬件数据流控制
	USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;					//收发模式

	USART_Init(USART2, &USART_InitStructure); //初始化串口1

	USART_Cmd(USART2, ENABLE); //使能串口1

	//USART_ClearFlag(USART1, USART_FLAG_TC);

	USART_ITConfig(USART2, USART_IT_RXNE, ENABLE); //开启相关中断

	//Usart1 NVIC 配置
	NVIC_InitStructure.NVIC_IRQChannel = USART2_IRQn;		  //串口1中断通道
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 3; //抢占优先级3
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3;		  //子优先级3
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;			  //IRQ通道使能
	NVIC_Init(&NVIC_InitStructure);							  //根据指定的参数初始化VIC寄存器、

	RecPack_Init(Bufs);
	Send_Str("OK\n");
}

void Send_Str(char *str)
{
	int i;
	for (i = 0; str[i] != 0; i++)
	{
		USART_SendData(USART2, str[i]);
	}
}

void Motor_Ctrl(int16_t pwm1, int16_t pwm2)
{

	int16_t pwmL;
	int16_t pwmR;
	switch (Speed)
	{
	case 'L':
		Speed_g = 300.0;
		break;
	case 'M':
		Speed_g = 180.0;
		break;
	case 'H':
		Speed_g = 100.0;
		break;
	}
	pwmL = ((float)pwm1 / Speed_g) * MOTOR_DUTY_MAX;
	if (pwm1 > 0)
	{

		user_device_pwm_duty_set(MOTOR_LEFT_SELECT, MOTOR_DIR_SELECT_TOWARD, pwmL);
	}
	else
	{
		user_device_pwm_duty_set(MOTOR_LEFT_SELECT, MOTOR_DIR_SELECT_BACKWARD, abs(pwmL));
	}
	pwmR = ((float)pwm2 / Speed_g) * MOTOR_DUTY_MAX;
	if (pwm2 > 0)
	{
		user_device_pwm_duty_set(MOTOR_RIGHT_SELECT, MOTOR_DIR_SELECT_TOWARD, pwmR);
	}
	else
	{
		user_device_pwm_duty_set(MOTOR_RIGHT_SELECT, MOTOR_DIR_SELECT_BACKWARD, abs(pwmR));
	}
}

#define INTERVAL 50
#define DELAY               \
	while (res_ctrl != 'S') \
	{                       \
		t--;                \
		goto OUT;           \
		if (t == 0)         \
			break;          \
		rt_thread_delay(4); \
	}                       \
	t = INTERVAL

void Crazy()
{
	int t = INTERVAL;
	Motor_Ctrl(10, 100);
	DELAY;

	Motor_Ctrl(100, 50);
	DELAY;
	Motor_Ctrl(-100, 100);
	Motor_Ctrl(100, -80);
	DELAY;
	Motor_Ctrl(-100, -30);
	DELAY;
OUT:;
}

void HandCtrl(void)
{
	switch (res_ctrl)
	{
	case 'Q':
		Motor_Ctrl(10, 100);
		break;
	case 'W':
		Motor_Ctrl(100, 100);
		break;
	case 'E':
		Motor_Ctrl(100, 10);
		break;
	case 'A':
		Motor_Ctrl(-100, 100);
		break;
	case 'S':
		Crazy();
		break;
	case 'D':
		Motor_Ctrl(100, -100);
		break;
	case 'Z':
		Motor_Ctrl(-10, -100);
		break;
	case 'X':
		Motor_Ctrl(-100, -100);
		break;
	case 'C':
		Motor_Ctrl(-100, -10);
		break;
	case 'R':
		Motor_Ctrl(0, 0);
		break;
	case 'L':
		Speed = 'L';
		break;
	case 'M':
		Speed = 'M';
		break;
	case 'H':
		Speed = 'H';
		break;
	}
}

void USART2_IRQHandler(void) //串口1中断服务程序
{

	if (USART_GetITStatus(USART2, USART_IT_RXNE) != RESET)
	{
		res_ctrl = USART_ReceiveData(USART2);

		rec = 1;
	}
}
